#include "rclcpp/rclcpp.hpp"
#include "robot_move_interface/msg/robot_pose.hpp"
#include "robot_move_interface/msg/robot_status.hpp"
#include "robot_move_interface/srv/move_robot.hpp"

class RobotMoveControl:public rclcpp::Node{
public:
    RobotMoveControl(std::string name):Node(name){
        RCLCPP_INFO(this->get_logger(),"I am robot move controller %s ...",name.c_str());
        client_=this->create_client<robot_move_interface::srv::MoveRobot>("move_robot");
        subscription_=this->create_subscription<robot_move_interface::msg::RobotStatus>("robot_topic",10,std::bind(&RobotMoveControl::_status_callback,this,std::placeholders::_1));
    }
    void move_robot(float distance){
        RCLCPP_INFO(this->get_logger(),"Request move robot...");
        while(!client_->wait_for_service(std::chrono::seconds(1))){
            if(!rclcpp::ok()){
                RCLCPP_ERROR(this->get_logger(),"Broken while wait for service.");
            }
            RCLCPP_INFO(this->get_logger(),"Waiting service working...");
        }
        auto request=std::make_shared<robot_move_interface::srv::MoveRobot::Request>();
        request->distance=distance;
        client_->async_send_request(request,std::bind(&RobotMoveControl::_reponse_callback,this,std::placeholders::_1));
    }
private:
    void _status_callback(const robot_move_interface::msg::RobotStatus::SharedPtr message){
        if(message->status==1){
            RCLCPP_INFO(this->get_logger(),"Get robot pose is: %f, status is moving...",message->pose);
        }else{
            RCLCPP_INFO(this->get_logger(),"Get robot pose is: %f, status is stoppped.",message->pose);
        }
    }
    void _reponse_callback(rclcpp::Client<robot_move_interface::srv::MoveRobot>::SharedFuture result_future){
        RCLCPP_INFO(this->get_logger(),"Get current robot pose is: %f",result_future.get()->pose);
    }

private:
    rclcpp::Client<robot_move_interface::srv::MoveRobot>::SharedPtr client_{};
    rclcpp::Subscription<robot_move_interface::msg::RobotStatus>::SharedPtr subscription_{};
};

int main(int argc,char** argv){
    rclcpp::init(argc,argv);
    auto node=std::make_shared<RobotMoveControl>("robot_controller_01");
    //创建线程，循环监测和执行回调
    node->move_robot(10);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}